#define _USE_MATH_DEFINES

#include <stdlib.h>
#include <iostream>
#include <cmath>
#include <vector>
#include <GL/glut.h>
#include "common.h"
#include "Simulator.h"
#include "Timing.h"
using namespace std;

static Simulator *gSim = NULL;

// Display globals
int gMainHandle;
int mx = 0;
int my = 0;

// Camera globals
const float CAMERAXL = -2.825;
const float CAMERAXR = 3.125;
const float CAMERAZL = -2.25;
const float CAMERAZR = 3.75;
//NxVec3 gCameraPos(0,5,-8);
//NxVec3 gCameraForward(0,0,1);
//NxVec3 gCameraRight(-1,0,0);
NxVec3 gCameraPos(CAMERAXL,6,CAMERAZL);
NxVec3 gCameraForward(1,-1,1);
NxVec3 gCameraRight(-1,0,0);
//NxVec3 gCameraPos(-0.6,1.2,-0.6);
//NxVec3 gCameraForward(1,-1,1);
//NxVec3 gCameraRight(-1,0,0);
const NxReal gCameraSpeed = 0.02;


// Keyboard globals
#define MAX_KEYS 256
bool gKeys[MAX_KEYS];
bool keyPressed = true;

// Mouse globals
bool firstClick = false;
bool buttonDown = false;

// Simulation globals
bool bPause = false;
GLuint texture;
// Selected shape
NxShape* selectedShape;
GLuint LoadTextureRAW( const char * filename, int wrap )
{
    GLuint texture;
    int width, height;
    void * data;
    FILE * file;

    // open texture data
    file = fopen( filename, "rb" );
    if ( file == NULL ) return 0;

    // allocate buffer
    width = 256;
    height = 256;
    data = malloc( width * height * 3 );

    // read texture data
    fread( data, width * height * 3, 1, file );
    fclose( file );

    // allocate a texture name
    glGenTextures( 1, &texture );

    // select our current texture
    glBindTexture( GL_TEXTURE_2D, texture );

    // select modulate to mix texture with color for shading
    glTexEnvf( GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_MODULATE );

    // when texture area is small, bilinear filter the closest mipmap
    glTexParameterf( GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER,
                     GL_LINEAR_MIPMAP_NEAREST );
    // when texture area is large, bilinear filter the first mipmap
    glTexParameterf( GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR );

    // if wrap is true, the texture wraps over at the edges (repeat)
    //       ... false, the texture ends at the edges (clamp)
    glTexParameterf( GL_TEXTURE_2D, GL_TEXTURE_WRAP_S,
                     wrap ? GL_REPEAT : GL_CLAMP );
    glTexParameterf( GL_TEXTURE_2D, GL_TEXTURE_WRAP_T,
                     wrap ? GL_REPEAT : GL_CLAMP );

    // build our texture mipmaps
    gluBuild2DMipmaps( GL_TEXTURE_2D, 3, width, height,
                       GL_RGB, GL_UNSIGNED_BYTE, data );

    // free buffer
    free( data );

    return texture;
}
void ProcessKeys()
{
	// Process keys
	for (int i = 0; i < MAX_KEYS; i++)
	{	
		if (!gKeys[i])  { continue; }
		
		switch (i)
		{
				// Camera controls
			case 'a':{
				if (keyPressed) {
					//printf("%i %i %i %i", CAMERAXL, CAMERAZL, static_cast<int>(gCameraPos.x), static_cast<int>(gCameraPos.z));
					if (static_cast<float>(gCameraPos.x) == CAMERAXL && static_cast<float>(gCameraPos.z) == CAMERAZL) {
						gCameraPos.x = CAMERAXR;
						gCameraForward = NxVec3(-1,-1,1);
					} else if (static_cast<float>(gCameraPos.x) == CAMERAXR && static_cast<float>(gCameraPos.z) == CAMERAZL) {
						gCameraPos.z = CAMERAZR;
						gCameraForward = NxVec3(-1,-1,-1);
					} else if (static_cast<float>(gCameraPos.x) == CAMERAXR && static_cast<float>(gCameraPos.z) == CAMERAZR) {
						gCameraPos.x = CAMERAXL;
						gCameraForward = NxVec3(1,-1,-1);
					} else {
						gCameraPos.z = CAMERAZL;
						gCameraForward = NxVec3(1,-1,1);
					}
					keyPressed = false;
				}
				break; }
			case 's':{
				if (keyPressed) {
					if (static_cast<float>(gCameraPos.x) == CAMERAXL && static_cast<float>(gCameraPos.z) == CAMERAZL) {
						gCameraPos.z = CAMERAZR;
						gCameraForward = NxVec3(1,-1,-1);
					} else if (static_cast<float>(gCameraPos.x) == CAMERAXR && static_cast<float>(gCameraPos.z) == CAMERAZL) {
						gCameraPos.x = CAMERAXL;
						gCameraForward = NxVec3(1,-1,1);
					} else if (static_cast<float>(gCameraPos.x) == CAMERAXR && static_cast<float>(gCameraPos.z) == CAMERAZR) {
						gCameraPos.z = CAMERAZL;
						gCameraForward = NxVec3(-1,-1,1);
					} else {
						gCameraPos.x = CAMERAXR;
						gCameraForward = NxVec3(-1,-1,-1);
					}
					keyPressed = false;
				}
				break; }
			//case 'w':{ gCameraPos += gCameraForward*gCameraSpeed; break; }
			//case 's':{ gCameraPos -= gCameraForward*gCameraSpeed; break; }
			//case 'a':{ gCameraPos -= gCameraRight*gCameraSpeed; break; }
			//case 'd':{ gCameraPos += gCameraRight*gCameraSpeed; break; }
			//case 'z':{ gCameraPos -= NxVec3(0,1,0)*gCameraSpeed; break; }
			case 'q':{ gSim->SetActor(gSim->mObjects[0]); }
				
		}
	}
}

void SetupCamera()
{
	// Setup camera
	glMatrixMode(GL_PROJECTION);
    glLoadIdentity();
    gluPerspective(60.0f, (float)glutGet(GLUT_WINDOW_WIDTH)/(float)glutGet(GLUT_WINDOW_HEIGHT), 1.0f, 10000.0f);
	gluLookAt(gCameraPos.x,gCameraPos.y,gCameraPos.z,gCameraPos.x + gCameraForward.x, gCameraPos.y + gCameraForward.y, gCameraPos.z + gCameraForward.z, 0.0f, 1.0f, 0.0f);
	
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
}

void RenderCallback()
{
    if (gSim && !bPause)    
        gSim->RunPhysics();
	
    // Clear buffers
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
	
	ProcessKeys();
	SetupCamera();
    if (gSim) {
		gSim->RenderScene();
		gSim->ProcessKeys(gKeys);
	}
	
    glutSwapBuffers();
}

void ReshapeCallback(int width, int height)
{
    glViewport(0, 0, width, height);
}

void IdleCallback()
{
    glutPostRedisplay();
}

void KeyboardCallback(unsigned char key, int x, int y)
{
	gKeys[key] = true;
	
	switch (key)
	{
		case 27 : { exit(0); break; }
		case 'p': { bPause = !bPause; getElapsedTime(); break; }
		default: { break; }
	}
}

static void ArrowKeyCallback(int key, int x, int y)
{
	KeyboardCallback(key,x,y);
}

void KeyboardUpCallback(unsigned char key, int x, int y)
{
	gKeys[key] = false;
	keyPressed = true;
}

void MouseCallback(int button, int state, int x, int y)
{
	firstClick = true;
	
	// Check if the button is being pressed or released
	buttonDown = !buttonDown;
	if (!buttonDown) return;
	
	// Determine position of mouse vector relative to the z axis
	float xPos = tan(30.0f * M_PI / 180) * (x - 256) / 256;
	float yPos = tan(30.0f * M_PI / 180) * -(y - 256) / 256;
	NxVec3 mouseVec(xPos,yPos,1);
	
	gCameraForward.normalize();
	mouseVec.normalize();
	
	NxVec3 norm(0,0,1);
	NxVec3 cam = gCameraForward;
	
	// Compute axis to rotate around
	NxVec3 cross(norm.y * cam.z - norm.z * cam.y,
		norm.z * cam.x - norm.x * cam.z,
		norm.x * cam.y - norm.y * cam.x);
	
	// Compute up angle relative to the camera
	NxVec3 camUp(cam.y * -cam.x,
		cam.z * cam.z - cam.x * -cam.x,
		-cam.y * cam.z);
	camUp.normalize();
	
	// Up angle relative to the z axis
	NxVec3 up(0,1,0);
	
	// Rotate axis onto the xz plane
	float theta = 0;
	if (cross.x != 0) theta = -atan((float) cross.y/cross.x);
	else theta = M_PI / 2;
	cross = NxVec3(cos(theta) * cross.x - sin(theta) * cross.y,0,cross.z);
	mouseVec = NxVec3(cos(theta) * mouseVec.x - sin(theta) * mouseVec.y,
		sin(theta) * mouseVec.x + cos(theta) * mouseVec.y,
		mouseVec.z);
	cam = NxVec3(cos(theta) * cam.x - sin(theta) * cam.y,
		sin(theta) * cam.x + cos(theta) * cam.y,
		cam.z);
	up = NxVec3(cos(theta) * up.x - sin(theta) * up.y,
		sin(theta) * up.x + cos(theta) * up.y,
		up.z);
	
	// Rotate axis onto the z-axis
	float theta2 = 0;
	if (cross.z != 0) theta2 = -atan((float) cross.x/cross.z);
	else theta2 = -M_PI / 2;
	cross = NxVec3(0,0,-sin(theta2) * cross.x + cos(theta2) * cross.z);
	norm = NxVec3(sin(theta2),0,cos(theta2));
	mouseVec = NxVec3(cos(theta2) * mouseVec.x + sin(theta2) * mouseVec.z,
		mouseVec.y,
		-sin(theta2) * mouseVec.x + cos(theta2) * mouseVec.z);
	cam = NxVec3(cos(theta2) * cam.x + sin(theta2) * cam.z,
		cam.y,
		-sin(theta2) * cam.x + cos(theta2) * cam.z);
	up = NxVec3(cos(theta2) * up.x + sin(theta2) * up.z,
		up.y,
		-sin(theta2) * up.x + cos(theta2) * up.z);
	
	// Rotate about z-axis
	float theta3 = 0;
	if (norm.x != 0)
		theta3 = asin(cam.x) - M_PI / 2 * (norm.x / abs(norm.x))/* + atan(norm.y / norm.x)*/;
	mouseVec = NxVec3(cos(theta3) * mouseVec.x - sin(theta3) * mouseVec.y,
		sin(theta3) * mouseVec.x + cos(theta3) * mouseVec.y,
		mouseVec.z);
	up = NxVec3(cos(theta3) * up.x - sin(theta3) * up.y,
		sin(theta3) * up.x + cos(theta3) * up.y,
		up.z);
	
	// Undo rotation onto z-axis
	theta2 = -theta2;
	mouseVec = NxVec3(cos(theta2) * mouseVec.x + sin(theta2) * mouseVec.z,
		mouseVec.y,
		-sin(theta2) * mouseVec.x + cos(theta2) * mouseVec.z);
	up = NxVec3(cos(theta2) * up.x + sin(theta2) * up.z,
		up.y,
		-sin(theta2) * up.x + cos(theta2) * up.z);
	
	// Undo rotation onto xz plane
	theta = -theta;
	mouseVec = NxVec3(cos(theta) * mouseVec.x - sin(theta) * mouseVec.y,
		sin(theta) * mouseVec.x + cos(theta) * mouseVec.y,
		mouseVec.z);
	up = NxVec3(cos(theta) * up.x - sin(theta) * up.y,
		sin(theta) * up.x + cos(theta) * up.y,
		up.z);
	
	// Reset camera vector
	cam = gCameraForward;
	
	// Rotate camera onto the xz plane
	theta = 0;
	if (cam.x != 0) theta = -atan((float) cam.y/cam.x);
	else theta = M_PI / 2;
	cam = NxVec3(cos(theta) * cam.x - sin(theta) * cam.y,
		sin(theta) * cam.x + cos(theta) * cam.y,
		cam.z);
	mouseVec = NxVec3(cos(theta) * mouseVec.x - sin(theta) * mouseVec.y,
		sin(theta) * mouseVec.x + cos(theta) * mouseVec.y,
		mouseVec.z);
	camUp = NxVec3(cos(theta) * camUp.x - sin(theta) * camUp.y,
		sin(theta) * camUp.x + cos(theta) * camUp.y,
		camUp.z);
	up = NxVec3(cos(theta) * up.x - sin(theta) * up.y,
		sin(theta) * up.x + cos(theta) * up.y,
		up.z);
	
	// Rotate camera onto the z-axis
	theta2 = 0;
	if (cam.z != 0) theta2 = -atan((float) cam.x/cam.z);
	else theta2 = -M_PI / 2;
	cam = NxVec3(cos(theta2) * cam.x + sin(theta2) * cam.z,
		cam.y,
		-sin(theta2) * cam.x + cos(theta2) * cam.z);
	mouseVec = NxVec3(cos(theta2) * mouseVec.x + sin(theta2) * mouseVec.z,
		mouseVec.y,
		-sin(theta2) * mouseVec.x + cos(theta2) * mouseVec.z);
	camUp = NxVec3(cos(theta2) * camUp.x + sin(theta2) * camUp.z,
		camUp.y,
		-sin(theta2) * camUp.x + cos(theta2) * camUp.z);
	up = NxVec3(cos(theta2) * up.x + sin(theta2) * up.z,
		up.y,
		-sin(theta2) * up.x + cos(theta2) * up.z);
	
	// Rotate about z-axis
	theta3 = 0;
	if (up.x != 0)
		theta3 = -(asin(camUp.x) - M_PI / 2 * (up.x / abs(up.x)) + atan(up.y / up.x));
	mouseVec = NxVec3(cos(theta3) * mouseVec.x - sin(theta3) * mouseVec.y,
		sin(theta3) * mouseVec.x + cos(theta3) * mouseVec.y,
		mouseVec.z);
	up = NxVec3(cos(theta3) * up.x - sin(theta3) * up.y,
		sin(theta3) * up.x + cos(theta3) * up.y,
		up.z);
	
	// Undo rotation onto z-axis
	theta2 = -theta2;
	mouseVec = NxVec3(cos(theta2) * mouseVec.x + sin(theta2) * mouseVec.z,
		mouseVec.y,
		-sin(theta2) * mouseVec.x + cos(theta2) * mouseVec.z);
	
	// Undo rotation onto xz plane
	theta = -theta;
	mouseVec = NxVec3(cos(theta) * mouseVec.x - sin(theta) * mouseVec.y,
		sin(theta) * mouseVec.x + cos(theta) * mouseVec.y,
		mouseVec.z);
	
	// Haxors
	mouseVec = NxVec3(mouseVec.z,mouseVec.y,mouseVec.x);
	if (gCameraPos.z < 0 && gCameraPos.x > 0 || gCameraPos.z > 0 && gCameraPos.x < 0)  {
		mouseVec.z = -mouseVec.z;
		mouseVec.x = -mouseVec.x;
	}
	
	// Fire ray from camera, return the shape hit.
	NxRay ray(gCameraPos,mouseVec);
	NxRaycastHit hit;
	selectedShape = gSim->mScene->raycastClosestShape(ray,NX_ALL_SHAPES,hit);
	
	// Set the movable shape to the actor
	gSim->SetActor(&(selectedShape->getActor()));
	
	mx = x;
	my = y;
}

void MotionCallback(int x, int y)
{
    int dx = mx - x;
    int dy = my - y;
    
    gCameraForward.normalize();
    gCameraRight.cross(gCameraForward,NxVec3(0,1,0));
	
    NxQuat qx(NxPiF32 * dx * 20 / 180.0f, NxVec3(0,1,0));
    qx.rotate(gCameraForward);
    NxQuat qy(NxPiF32 * dy * 20 / 180.0f, gCameraRight);
    qy.rotate(gCameraForward);
	
    mx = x;
    my = y;
}

void PassiveMotionCallback(int x, int y)
{
	int dx = -mx + x;
	int dy = my - y;
	
	mx = x;
	my = y;
	
	if (firstClick)
		gSim->ProcessMouseMotion(dx, dy, gCameraPos);
}

void ExitCallback()
{
	gSim->ReleaseNx();
}

void InitGlut(int argc, char **argv)
{
    glutInit(&argc, argv);
    glutInitWindowSize(512, 512);
    glutInitDisplayMode(GLUT_RGB | GLUT_DOUBLE | GLUT_DEPTH);
	gMainHandle = glutCreateWindow("Simulation");
	glutSetWindow(gMainHandle);
    glutDisplayFunc(RenderCallback);
    glutReshapeFunc(ReshapeCallback);
    glutIdleFunc(IdleCallback);
    glutKeyboardFunc(KeyboardCallback);
	glutSpecialFunc(ArrowKeyCallback);
    glutKeyboardUpFunc(KeyboardUpCallback);
    glutMouseFunc(MouseCallback);
	glutPassiveMotionFunc(PassiveMotionCallback);
    //glutMotionFunc(MotionCallback);
	MotionCallback(0,0);
    // Setup default render states
	//enable texture

	glEnable(GL_TEXTURE_2D);
	texture = LoadTextureRAW("texture.raw",0);
	glBindTexture(GL_TEXTURE_2D,texture);
	glClearColor(0.52f, 0.60f, 0.71f, 1.0f);  
    glEnable(GL_DEPTH_TEST);
    glEnable(GL_COLOR_MATERIAL);
	
    // Setup lighting
    glEnable(GL_LIGHTING);
    float AmbientColor[]    = { 0.0f, 0.1f, 0.2f, 0.0f };         glLightfv(GL_LIGHT0, GL_AMBIENT, AmbientColor);
    float DiffuseColor[]    = { 0.2f, 0.2f, 0.2f, 0.0f };         glLightfv(GL_LIGHT0, GL_DIFFUSE, DiffuseColor);
    float SpecularColor[]   = { 0.5f, 0.5f, 0.5f, 0.0f };         glLightfv(GL_LIGHT0, GL_SPECULAR, SpecularColor);
    float Position[]        = { 100.0f, 100.0f, -400.0f, 1.0f };  glLightfv(GL_LIGHT0, GL_POSITION, Position);
    glEnable(GL_LIGHT0);
}
 
int main(int argc, char** argv)
{
    InitGlut(argc, argv);
	
	gSim = new Simulator();
	// Initialize physics scene and start the application main loop if scene was created
	if (gSim->InitNx()){
		gSim->CreateScene();
		atexit(ExitCallback);
		glutMainLoop();
	}
	delete gSim;
	return 0;
}
